/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "JCMaCISpeedCtrl.h"
#include <assert.h>

JCMaCISpeedCtrl::JCMaCISpeedCtrl(gimi::GIMI *aGIMIPtr, 
                                 wxSizer *compSizer,
				 wxPanel *aComponentPanel)
  :  JoystickControlComponent (aGIMIPtr, compSizer, aComponentPanel, MSC_NAME,
                              GIMI_PROTOCOL_MACI_SPEEDCTRL, -1),
     iSpeedCtrlClient(NULL),
     iVehicleMaxSpeed(0.5),
     iVehicleMaxAngularSpeed(1),
     iVehicleMaxAcceleration(0.0),
     iCommandThread(NULL),
     iTarget()
{
  // Add options here.
  configOptions.addAxisOption(SPEEDCTRL_STEERINGJOYSTICK_AXIS, "Steering with joystick");
  configOptions.addAxisOption(SPEEDCTRL_GASBRAKEJOYSTICK_AXIS, "Gas/brakes with joystick");
  configOptions.addAxisOption(SPEEDCTRL_STEERING_WHEEL_AXIS, "Steering with steering wheel");
  configOptions.addAxisOption(SPEEDCTRL_GASPEDAL_AXIS, "Gas with gas/brake pedals");
  configOptions.addAxisOption(SPEEDCTRL_GASBRAKEPEDAL_AXIS, "Gas/brakes with gas/brake pedals");
  configOptions.addButtonOption(SPEEDCTRL_HANDBRAKE_BUTTON, "Handbrake");
  configOptions.addButtonOption(SPEEDCTRL_GEARUP_BUTTON, "Gear up");
  configOptions.addButtonOption(SPEEDCTRL_GEARDOWN_BUTTON, "Gear down");
  configOptions.addButtonOption(SPEEDCTRL_ACCUP_BUTTON, "Acceleration up");
  configOptions.addButtonOption(SPEEDCTRL_ACCDOWN_BUTTON, "Acceleration down");
}
     
JCMaCISpeedCtrl::~JCMaCISpeedCtrl()
{
void *param;
  // Stop the commandsenderthread gracefully.
  activationStatus.deactivate();
  if(iSpeedCtrlClient != NULL){
    iSpeedCtrlClient->Close();
    
    delete iSpeedCtrlClient;
  }
  if (iCommandThread != NULL) {
    ownThread_Join(iCommandThread, &param);
    iCommandThread = NULL;
  }
}

bool JCMaCISpeedCtrl::startComponent()
{
  bool result = false;
  dPrint(1, "Activation status %d", activationStatus.isActive());
  if (activationStatus.isActive()) {
    dPrint(2, "Component JCMaCISpeedCtrl is already activated!");
    result = false;
  } else {
    dPrint(2, "Starting component MaCISpeedControl");
    
    if (!iTarget.clientName.empty()) {
      assert(iGIMIPtr != NULL);
      iSpeedCtrlClient = new MaCI::SpeedCtrl::CSpeedCtrlClient(iGIMIPtr,iTarget.minorServiceId);
      
      if (iSpeedCtrlClient != NULL){
	
	iSpeedCtrlClient->SetDefaultTarget(iTarget.clientName);
	if(iSpeedCtrlClient->Open() == MaCI::KMaCIOK){
	    
	  //Should get max speed, when speedctrlclient provide this informatio

	  // Draw information stuff.
          wxFlexGridSizer* fgSizer1;
          fgSizer1 = new wxFlexGridSizer( 2, 2, 0, 0 );
          fgSizer1->SetFlexibleDirection( wxVERTICAL );
          fgSizer1->SetNonFlexibleGrowMode( wxFLEX_GROWMODE_SPECIFIED );

	  wxStaticText *compNameText = new wxStaticText(componentPanel, wxID_ANY,
              wxT("SpeedCtrl"), wxDefaultPosition, wxDefaultSize, 0);
          wxStaticText *compActText = new wxStaticText(componentPanel, wxID_ANY,
              wxT("Active"), wxDefaultPosition, wxDefaultSize, 0);
          
          textSpeedText = new wxStaticText(componentPanel, wxID_ANY,
              wxT("Max. speed:"), wxDefaultPosition, wxDefaultSize, 0);
          textSpeedValue = new wxStaticText(componentPanel, wxID_ANY,
              wxT("0.00"), wxDefaultPosition, wxDefaultSize, 0);
          textAngularSpeedText = new wxStaticText(componentPanel, wxID_ANY,
              wxT("Max. angularspeed:"), wxDefaultPosition, wxDefaultSize, 0);
          textAngularSpeedValue = new wxStaticText(componentPanel, wxID_ANY,
              wxT("0.00"), wxDefaultPosition, wxDefaultSize, 0);
	  textAccText = new wxStaticText(componentPanel, wxID_ANY,
              wxT("Max. acceleration:"), wxDefaultPosition, wxDefaultSize, 0);
          textAccValue = new wxStaticText(componentPanel, wxID_ANY,
              wxT("0.00"), wxDefaultPosition, wxDefaultSize, 0);
          textBrakeText = new wxStaticText(componentPanel, wxID_ANY,
              wxT("Handbrake: "), wxDefaultPosition, wxDefaultSize, 0);
          textBrakeValue = new wxStaticText(componentPanel, wxID_ANY,
              wxT("On"), wxDefaultPosition, wxDefaultSize, 0);
          textGearText = new wxStaticText(componentPanel, wxID_ANY,
              wxT("Gear:"), wxDefaultPosition, wxDefaultSize, 0);
          textGearValue = new wxStaticText(componentPanel, wxID_ANY,
              wxT("Forward"), wxDefaultPosition, wxDefaultSize, 0);
          textCurrentSpeedText = new wxStaticText(componentPanel, wxID_ANY,
              wxT("Speed:"), wxDefaultPosition, wxDefaultSize, 0);
          textCurrentSpeedValue = new wxStaticText(componentPanel, wxID_ANY,
              wxT("0.00"), wxDefaultPosition, wxDefaultSize, 0);
          textCurrentAngularSpeedText = new wxStaticText(componentPanel, wxID_ANY,
              wxT("Angularspeed:"), wxDefaultPosition, wxDefaultSize, 0);
          textCurrentAngularSpeedValue = new wxStaticText(componentPanel, wxID_ANY,
              wxT("0.00"), wxDefaultPosition, wxDefaultSize, 0);
          textCurrentAccelerationText = new wxStaticText(componentPanel, wxID_ANY,
              wxT("Acceleration:"), wxDefaultPosition, wxDefaultSize, 0);
          textCurrentAccelerationValue = new wxStaticText(componentPanel, wxID_ANY,
              wxT("0.00"), wxDefaultPosition, wxDefaultSize, 0);

          fgSizer1->Add(compNameText, 1 ,wxALL, 5);
          fgSizer1->Add(compActText, 1, wxALL, 5);
    
          fgSizer1->Add(textSpeedText, 1, wxALL, 5);
          fgSizer1->Add(textSpeedValue, 1, wxALL, 5);
          fgSizer1->Add(textAngularSpeedText, 1, wxALL, 5);
          fgSizer1->Add(textAngularSpeedValue, 1, wxALL, 5);
	  fgSizer1->Add(textAccText, 1, wxALL, 5);
          fgSizer1->Add(textAccValue, 1, wxALL, 5);
          fgSizer1->Add(textBrakeText, 1, wxALL, 5);
          fgSizer1->Add(textBrakeValue, 1, wxALL, 5);
          fgSizer1->Add(textGearText, 1, wxALL, 5);
          fgSizer1->Add(textGearValue, 1, wxALL, 5);
          fgSizer1->Add(textCurrentSpeedText, 1, wxALL, 5);
          fgSizer1->Add(textCurrentSpeedValue, 1, wxALL, 5);
          fgSizer1->Add(textCurrentAngularSpeedText, 1, wxALL, 5);
          fgSizer1->Add(textCurrentAngularSpeedValue, 1, wxALL, 5);          
	  fgSizer1->Add(textCurrentAccelerationText, 1, wxALL, 5);
          fgSizer1->Add(textCurrentAccelerationValue, 1, wxALL, 5);      
          componentSizer->Add(fgSizer1, 1, wxEXPAND, 5);
          
          activationStatus.activate();
          iCommandThread = ownThread_Create((void *)(commandSenderThread), this);
          result = true;
          dPrint(1, "JCMaCISpeedCtrl activated.");
          dPrint(1, "Activation status is %d", activationStatus.isActive());



	} else { // Failed to initialize JointSpeedControl
	  dPrint(1, "Failed to Open MaCI SpeedCtrl-client");
	}
	
	
      }else{
	dPrint(1,"Failed to Create SpeedCtrl client");
      }
    }else { // No targets.
      dPrint(1, "Cannot start component without targets.");
    }

  }

  return result;
}

int JCMaCISpeedCtrl::addTarget(const gimi::GIMIService &aTarget)
{
  iTarget = aTarget;
  return JOYSTICKCONTROL_OK;
}


void JCMaCISpeedCtrl::commandSenderThread(void *p)
{
  dPrint(1, "Started");
  
  JCMaCISpeedCtrl *control =
      static_cast<JCMaCISpeedCtrl *>(p);

  gim::time current, nextrun, timediff(50), lastCommand;

  //TODO FIXME Get these from SpeedCtrlClient when implemented!
  MaCI::SpeedCtrl::TSpeed maxSpeeds;
  MaCI::SpeedCtrl::TSpeed minSpeeds;
  if(control->iSpeedCtrlClient->GetSpeedLimits(maxSpeeds,minSpeeds)){
    control->iVehicleMaxSpeed = maxSpeeds.speed;
    control->iVehicleMinSpeed = minSpeeds.speed;
    control->iVehicleMaxAngularSpeed = maxSpeeds.angularSpeed;
    control->iVehicleMaxAcceleration = maxSpeeds.acceleration;

  }else{
    //If max speed are not set, use J2B2 values...
    control->iVehicleMaxSpeed = 0.5;
  // Avant 4.1
  // j2b2 0.50
    control->iVehicleMaxAngularSpeed = 3.8;
  // Avant 1.8
  // j2b2 3.80
    control->iVehicleMaxAcceleration = 0.5;
  }

  float acc = control->iVehicleMaxAcceleration/2.5;

  // Update speeds to text
  char speedStr[32];
  char angSpeedStr[32];
  char accStr[32];
  snprintf(speedStr, 30, "%.2f", control->iVehicleMaxSpeed);
  snprintf(angSpeedStr, 30, "%.2f", control->iVehicleMaxAngularSpeed);
  snprintf(accStr, 30, "%.2f", control->iVehicleMaxAcceleration);
  wxMutexGuiEnter();
  control->textSpeedValue->SetLabel(wxT(speedStr));
  control->textAngularSpeedValue->SetLabel(wxT(angSpeedStr));
  control->textAccValue->SetLabel(wxT(accStr));
  snprintf(accStr, 30, "%.2f", acc);
  control->textCurrentAccelerationValue->SetLabel(wxT(accStr));
  wxMutexGuiLeave();

  int gear = 1; ///< -1 for reverse, 0 for idle, 1+ for forward.
  bool brakeState = true; ///< True for brakes on, false for brakes off.
  bool driveStateChanged = false; ///< Send new setSpeed if gear or brakes have
                                  ///< changed state (even if axes have not)
  
  // Send current status at some interval, if status has been changed.
  // Send changes in buttons immediately (brakes and gears)
  while (control->activationStatus.isActive()) {

    //// Buttons
    bool buttonDown;
    // Brakes
    if (control->configOptions.readButtonOption(SPEEDCTRL_HANDBRAKE_BUTTON, &buttonDown) ==
        JOYSTICKCONTROL_STATUS_CHANGED && buttonDown)
    {
      dPrint(3, "Handbrake button down");
      if (brakeState) { // Brakes are on, send start.
        dPrint(2, "Brakes were on, sending start.");
        brakeState = false;
	//        control->vehicleD2DClient->Start();
        driveStateChanged = true;
        
        wxMutexGuiEnter();
        control->textBrakeValue->SetLabel(wxT("Off"));
        wxMutexGuiLeave();
        
      } else { // Brakes are off, send stop.
        dPrint(2, "Brakes were off, sending stop.");
        brakeState = true;
        control->iSpeedCtrlClient->SetStop();
        driveStateChanged = true;
        
        wxMutexGuiEnter();
        control->textBrakeValue->SetLabel(wxT("On"));
        control->textCurrentSpeedValue->SetLabel(wxT("0.00"));
        control->textCurrentAngularSpeedValue->SetLabel(wxT("0.00"));
        wxMutexGuiLeave();
      }
    }


    // Gear up
    if (control->configOptions.readButtonOption(SPEEDCTRL_GEARUP_BUTTON, &buttonDown) ==
        JOYSTICKCONTROL_STATUS_CHANGED && buttonDown)
    {
      dPrint(3, "Setting gear forward.");
      gear = 1;
      driveStateChanged = true;
      
      wxMutexGuiEnter();
      control->textGearValue->SetLabel(wxT("Forward"));
      wxMutexGuiLeave();
    }
    // Gear down
    if (control->configOptions.readButtonOption(SPEEDCTRL_GEARDOWN_BUTTON, &buttonDown) ==
        JOYSTICKCONTROL_STATUS_CHANGED && buttonDown)
    {
      dPrint(3, "Setting gear reverse.");
      gear = -1;
      driveStateChanged = true;
      
      wxMutexGuiEnter();
      control->textGearValue->SetLabel(wxT("Reverse"));
      wxMutexGuiLeave();
    }
    // Acceleration up
    if (control->configOptions.readButtonOption(SPEEDCTRL_ACCUP_BUTTON, &buttonDown) ==
        JOYSTICKCONTROL_STATUS_CHANGED && buttonDown)
    {
      dPrint(3, "Setting more Acceleration.");
      acc +=0.05;
      if(acc > control->iVehicleMaxAcceleration){
	acc = control->iVehicleMaxAcceleration;
      }
      driveStateChanged = true;
      snprintf(accStr, 30, "%.2f", acc);
      wxMutexGuiEnter();
      control->textCurrentAccelerationValue->SetLabel(wxT(accStr));
     
      wxMutexGuiLeave();
    }
    // Acceleration up
    if (control->configOptions.readButtonOption(SPEEDCTRL_ACCDOWN_BUTTON, &buttonDown) ==
        JOYSTICKCONTROL_STATUS_CHANGED && buttonDown)
    {
      dPrint(3, "Setting more Acceleration.");
      acc -=0.05;
      if(acc <0.0){
	acc = 0.0;
      }
      driveStateChanged = true;
      snprintf(accStr, 30, "%.2f", acc);
      wxMutexGuiEnter();
      control->textCurrentAccelerationValue->SetLabel(wxT(accStr));
     
      wxMutexGuiLeave();
    }
    //// Send axis events every ~50ms
    current.setToCurrent();
    if (current > nextrun) {
      nextrun = current + timediff;
      float steeringAxisValue;
      float gasPedalAxisValue;
      bool steeringWheelInUse = false;
      bool gasPedalInUse = false;
      bool gasBrakePedalInUse = false;
      steeringWheelInUse =control->isAxisOptionIdSet(SPEEDCTRL_STEERING_WHEEL_AXIS);

      int steeringChanged;
      if(!steeringWheelInUse){
      
       steeringChanged = control->configOptions.readAxisOption(SPEEDCTRL_STEERINGJOYSTICK_AXIS, &steeringAxisValue);
      }else{
     
        steeringChanged = control->configOptions.readAxisOption(SPEEDCTRL_STEERING_WHEEL_AXIS, &steeringAxisValue);
      }
      int gasChanged;
      gasPedalInUse = control->isAxisOptionIdSet(SPEEDCTRL_GASPEDAL_AXIS);
      gasBrakePedalInUse = control->isAxisOptionIdSet(SPEEDCTRL_GASBRAKEPEDAL_AXIS);      
      if(!gasPedalInUse){
        if(gasBrakePedalInUse){
          gasChanged = control->configOptions.readAxisOption(SPEEDCTRL_GASBRAKEPEDAL_AXIS, &gasPedalAxisValue);
        }else{
          gasChanged = control->configOptions.readAxisOption(SPEEDCTRL_GASBRAKEJOYSTICK_AXIS, &gasPedalAxisValue);
        }
      }else{
    
        gasChanged = control->configOptions.readAxisOption(SPEEDCTRL_GASPEDAL_AXIS, &gasPedalAxisValue);
      }



      if (steeringChanged == JOYSTICKCONTROL_STATUS_CHANGED ||
          gasChanged == JOYSTICKCONTROL_STATUS_CHANGED ||
          driveStateChanged ||
          current - lastCommand >100)
      {
        
        float turnValue = -steeringAxisValue;
        float gasValue = gasPedalAxisValue;
        dPrint(10,"turnvalue %f",turnValue);
        if (gasValue > 0.01 || gasValue < 0.01 || fabs(turnValue) > 0.05&& !brakeState) { // Gas-pedal pressed and no brakes applied.
        
          float expAdjustedTurn;
          float expAdjustedGas;

          // Adjust pedal curve. (^2 for smoother movement)
          expAdjustedGas = gasValue * gasValue;          
          //If using brake pedal AND the gas value is negative... then send negative values...
          if(gasBrakePedalInUse && gasValue < 0){
            expAdjustedGas = -expAdjustedGas;
          }
          // Adjust steering curve. (^2 for smoother turning)
          if (turnValue < 0) {
            expAdjustedTurn = -turnValue * turnValue;
          } else {
            expAdjustedTurn = turnValue * turnValue;
          }
          
          // Gas pedal affects turning speed.
          float angularSpeed = control->iVehicleMaxAngularSpeed * expAdjustedTurn;;
          /* if(gasPedalInUse){
            angularSpeed = control->iVehicleMaxAngularSpeed *
              expAdjustedTurn * gasValue;
          }else{
            
            // Gas pedal no longer affects turning speed.
            angularSpeed = control->iVehicleMaxAngularSpeed *
              expAdjustedTurn;
          }
          */



          float directSpeed;
          /*
          if(gasPedalInUse){          
          // More turning reduces direct speed.
          
            directSpeed = control->iVehicleMaxSpeed * expAdjustedGas *
              (1 - fabs(turnValue));
              
          }else{
            // Turning does not affect speed.
            directSpeed = control->iVehicleMaxSpeed * expAdjustedGas;
          }
          */

          if(expAdjustedGas > 0){
            directSpeed = control->iVehicleMaxSpeed * expAdjustedGas;
          }else{
            directSpeed = control->iVehicleMinSpeed * -expAdjustedGas;
          }

          // Limit value to avoid irregular movement
          if (directSpeed > control->iVehicleMaxSpeed && gear > 0) directSpeed = control->iVehicleMaxSpeed;
          if (directSpeed < control->iVehicleMinSpeed && gear < 0) directSpeed = control->iVehicleMinSpeed;
          if (directSpeed < control->iVehicleMinSpeed && gear < 0) directSpeed = control->iVehicleMinSpeed;
          if (directSpeed > control->iVehicleMaxSpeed && gear > 0) directSpeed = control->iVehicleMaxSpeed;

          if (angularSpeed > control->iVehicleMaxAngularSpeed) angularSpeed = control->iVehicleMaxAngularSpeed;

          if (gear > 0) { // Move forward.

            dPrint(6, "Set turn %.2f and speed %.2f for robot.", angularSpeed,
                directSpeed);
            control->iSpeedCtrlClient->SetSpeed(directSpeed, angularSpeed,acc);
            driveStateChanged = false;
            lastCommand.setToCurrent(); 
            snprintf(speedStr, 30, "%.2f", directSpeed);
            snprintf(angSpeedStr, 30, "%.2f", angularSpeed);
            wxMutexGuiEnter();
            control->textCurrentSpeedValue->SetLabel(wxT(speedStr));
            control->textCurrentAngularSpeedValue->SetLabel(wxT(angSpeedStr));
            wxMutexGuiLeave();

          } else if (gear < 0) { // Move backward.

            directSpeed = -directSpeed;
            angularSpeed = -angularSpeed;

            dPrint(6, "Set turn %.2f and speed %.2f for robot.", angularSpeed,
                   directSpeed);
            control->iSpeedCtrlClient->SetSpeed(directSpeed, angularSpeed,acc);
            lastCommand.setToCurrent(); 
            driveStateChanged = false;
            
            snprintf(speedStr, 30, "%.2f", directSpeed);
            snprintf(angSpeedStr, 30, "%.2f", angularSpeed);
            wxMutexGuiEnter();
            control->textCurrentSpeedValue->SetLabel(wxT(speedStr));
            control->textCurrentAngularSpeedValue->SetLabel(wxT(angSpeedStr));
            wxMutexGuiLeave();
          }
        
        } else { // No gas or brake-pedal pressed.
          control->iSpeedCtrlClient->SetSpeed(0, 0,acc);
          lastCommand.setToCurrent(); 
          driveStateChanged = false;
          
          wxMutexGuiEnter();
          control->textCurrentSpeedValue->SetLabel(wxT("0.00"));
          control->textCurrentAngularSpeedValue->SetLabel(wxT("0.00"));
          wxMutexGuiLeave();
        }
      }
    }
    
    ownSleep_ms(5);
  }
  
  dPrint(1, "Stopping");
  
  return;


}
